{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Urban POMDP tutorial"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This tutorial explains how to use the Urban POMDP model as a reinforcement learning environment. \n",
    "The source code can be found in [https://github.com/sisl/AutomotivePOMDPs/tree/master/src/generative_pomdps/urban]()."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "using Random\n",
    "using POMDPs\n",
    "using POMDPPolicies\n",
    "using POMDPSimulators\n",
    "using BeliefUpdaters\n",
    "using AutomotiveDrivingModels\n",
    "using AutomotiveSensors\n",
    "using AutomotivePOMDPs\n",
    "using AutoViz\n",
    "using Reel"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Define a random number generator to have reproducible simulations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "rng = MersenneTwister(1);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Initializing the environment"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The scenario geometry and road topology is defined using the `UrbanParams` and `UrbanEnv` types. To check the fields of these objects use the command `fieldnames`. \n",
    "\n",
    "The urban environment is alway a T shape intersections with crosswalk. \n",
    "\n",
    "Here we define a single lane intersection with three crosswalks and set the stop line position."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "params = UrbanParams(nlanes_main=1,\n",
    "                     crosswalk_pos =[VecSE2(6, 0., pi/2), VecSE2(-6, 0., pi/2), VecSE2(0., -5., 0.)],\n",
    "                     crosswalk_length =  [14.0, 14., 14.0],\n",
    "                     crosswalk_width = [4.0, 4.0, 3.1],\n",
    "                     stop_line = 22.0)\n",
    "env = UrbanEnv(params=params);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Initialize the pomdp object. The pomdp is implemented according to the [POMDPs.jl generative interface](http://juliapomdp.github.io/POMDPs.jl/latest/generative.html).\n",
    "\n",
    "It has various parameters:\n",
    "- sensor: defines the type of sensor that you want to use, for sensor models see [AutomotiveSensors.jl](https://github.com/sisl/AutomotiveSensors.jl)\n",
    "- ego_goal: the lane that the ego vehicle must reach\n",
    "- max_cars: the maximum number of cars supported by the simulations (will define the dimension of the observation vector)\n",
    "- max_peds: the maximum number of pedestrian\n",
    "- max_obstacles: the maximum number of obstacles on the side of the road. Maximum 3 obstacles are supported. They are randomly generated according to an obstacle distribution (see pomdp.obs_dist)\n",
    "- ego_start: the ego vehicle starting position as a longitudinal distance in the initial lane. \n",
    "- $\\Delta$T: the decision frequency"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "pomdp = UrbanPOMDP(env=env,\n",
    "                   sensor = PerfectSensor(),\n",
    "                   ego_goal = LaneTag(2, 1),\n",
    "                   max_cars=5, \n",
    "                   max_peds=5, \n",
    "                   car_birth=0.5, \n",
    "                   ped_birth=0.2, \n",
    "                   max_obstacles=0, # no fixed obstacles\n",
    "                   ego_start=20,\n",
    "                   ΔT=0.1);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Running simulation\n",
    "\n",
    "One can run simulations using the [POMDPSimulators.jl](https://github.com/JuliaPOMDP/POMDPSimulators.jl) package. See [this notebook](https://github.com/JuliaPOMDP/POMDPExamples.jl/blob/master/notebooks/Running-Simulations.ipynb) for how to use it. Here we run a simple simulation with a `HistoryRecorder` and a single action policy. \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "  5.738164 seconds (7.14 M allocations: 371.955 MiB, 6.87% gc time)\n"
     ]
    }
   ],
   "source": [
    "still_policy = FunctionPolicy(s -> UrbanAction(0.)) # no acceleration\n",
    "up = NothingUpdater() # no belief update, to use the observation as input to the policy see PreviousObservationUpdater from BeliefUpdaters.jl\n",
    "s0 = initialstate(pomdp, rng) # generate an initial state\n",
    "# set up the simulation\n",
    "hr = HistoryRecorder(rng=rng, max_steps=100)\n",
    "@time hist = simulate(hr, pomdp, still_policy, up, nothing, s0);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualize simulation\n",
    "\n",
    "To visualize a simulation we can generate a video using Reel.jl and the states stored in the history recorder."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "<video autoplay controls><source src=\"files/reel-473997054147348532.webm?11238503309942432567\" type=\"video/webm\"></video>"
      ],
      "text/plain": [
       "Frames{MIME{Symbol(\"image/png\")}}(\"C:\\\\Users\\\\Maxime\\\\AppData\\\\Local\\\\Temp\\\\jl_z0C55C\", 0x0000000000000065, 20.0, nothing)"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state_history = state_hist(hist)\n",
    "duration, fps, render_rec = animate_scenes(state_history, env, cam=StaticCamera(VecE2(0., -8.), 12.0))\n",
    "speed_factor = 2\n",
    "film = roll(render_rec, fps = speed_factor*fps, duration = duration/speed_factor)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "write(\"urbanpomdp.gif\", film);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## State and Observations"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The state is represented by a `Scene` from [AutomotiveDrivingModels.jl](https://github.com/sisl/AutomotiveDrivingModels.jl). It is a datastructure containing the physical state of all the traffic participants (global frame position, frenet frame position, velocity, shape of the vehicle, id). \n",
    "\n",
    "The observation is represented by a `Vector{Float64}`. It is of dimension: $n_{features}\\times(1 +max\\_cars + max\\_peds + max\\_obstacles)$\n",
    "\n",
    "$n_{features} = 4$ \n",
    "\n",
    "- dims 1 to 4 are the ego vehicle state as ego_pos_x, ego_pos_y, theta, v \n",
    "- then comes each car state as pos_x - ego_pos_x, pos_y - ego_pos_y, theta, v\n",
    "- then pedestrians states: pos_x - ego_pos_x, pos_y - ego_pos_y, theta, v\n",
    "- then obstacles states: widht, height, center_x - ego_pos_x, center_y - ego_pos_y\n",
    "\n",
    "Each quantity is normalized to be between 0 and 1."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Interact with the environment\n",
    "\n",
    "To interact with the environment one can use the function from the POMDPs.jl generative interface (see [doc](http://juliapomdp.github.io/POMDPs.jl/latest/generative.html))\n",
    "\n",
    "Another alternative is to use the [RLInterface.jl](https://github.com/JuliaPOMDP/RLInterface.jl) package that provide the ability to interface with existing python solvers. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "@webio": {
   "lastCommId": null,
   "lastKernelId": null
  },
  "kernelspec": {
   "display_name": "Julia 1.2.0",
   "language": "julia",
   "name": "julia-1.2"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "1.2.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
